﻿using Modbus.Device;
using System;
using System.Collections.Generic;
using System.IO.Ports;
using System.Linq;
using System.Net;
using System.Net.Sockets;
using System.Runtime.Versioning;
using System.Text;
using System.Threading.Tasks;

namespace QbotAGV
{
    public class KincoAxis // 导程10mm，电机转360度丝杆运动10mm
    {
        public IModbusSerialMaster master { get; set; }
        public SerialPort serial { get; private set; }
        public TcpClient tcp { get; private set; }
        public IPAddress ServerIp { get; set; }
        public int ServerPort { get; set; }
        public byte ModbusId { get; set; }
        public class ModbusAddress
        {
            public ushort TargetSpeed { get; set; } = 0x6F00;
            public ushort StatusWord { get; set; } = 0x3200;
            public ushort ControlWord { get; set; } = 0x3100;
            public ushort ActualCurrent { get; set; } = 0x3E00;
            public ushort ActualPos { get; set; } = 0x3700;
            public ushort AxisMode { get; set; } = 0x3500;  //1-位置模式，3-带位置环的速度模式，-3立即速度模式
            public ushort PosSpeed { get; set; } = 0x4A00;  //运行位置模式时候的速度
        }
        public class AxisModeList
        {
            public const byte PosMode = 1;
            public const byte VelMode = 0xFD;
        }
        public class ModbusValue
        {
            public const ushort PowerOn = 0x000F;
            public const ushort PowerOff = 0x0006;
            public const ushort PowrOnAtImmediatePos = 0x103F;
            public const ushort PowerOnAtPositionMode = 0x002F;
            public const double CurrentRatio = 42.6615969581749; //内部16进制和真实电流的换算关系，在手册上没找到，试出来的
            public ushort Status { get; set; } = 0;
            public int CmdVel { get; set; } = 0; //设定的速度[rpm]
            public int MaxVel { get; set; } = 500;
            public int ActualPos { get; set; } = 0;
            public byte AxisMode { get; set; } = AxisModeList.VelMode; //-1:0xFF,-2:0xFE,-3:0xFD
            public double AcutalCurrent { get; set; } = 0;
        }

        public static ModbusAddress Adrress { get; set; } = new ModbusAddress();
        public ModbusValue ServoValue { get; set; } = new ModbusValue();
        public bool ModbusOnline
        {
            get
            {
                return serial.IsOpen & !(master is null);
            }
        }

        public KincoAxis(IPAddress ip, int port, int modbus_id)
        {
            tcp = new TcpClient
            {
                ReceiveTimeout = 200
            };
            ModbusId = (byte)modbus_id;
            ServerIp = ip;
            ServerPort = port;
            master = ModbusSerialMaster.CreateRtu(tcp);
        }

        public KincoAxis(string com, int modbus_id)
        {
            serial = new SerialPort(com)
            {
                BaudRate = 115200,
                ReadTimeout = 100
            };
            ModbusId = (byte)modbus_id;
            master = ModbusSerialMaster.CreateRtu(serial);
        }

        public void Begin()
        {
            try
            {
                if (tcp is null)
                    serial.Open();
                tcp.Connect(ServerIp, ServerPort);
                // create modbus master
                ushort[] registers = master.ReadHoldingRegisters(ModbusId, Adrress.StatusWord, 1);
                ServoValue.Status = registers[0];
            }
            catch (Exception)
            {
                if (tcp is null)
                    serial.Close();
                else
                    tcp.Close();
                throw;
            }
        }

        public bool SetVelocity(int vel)
        {
            try
            {
                if (Math.Abs(vel) > ServoValue.MaxVel)
                    throw new Exception($"Vel: {vel} Exceed Maxvel: {ServoValue.MaxVel}");
                ServoValue.CmdVel = vel;
                var servo_cmd_vel = (int)(vel * 512 * 10000.0 / 1875);
                ushort[] registers = new ushort[2] { LowWord((uint)servo_cmd_vel), HighWord((uint)servo_cmd_vel) };
                master.WriteMultipleRegisters(ModbusId, Adrress.TargetSpeed, registers);
                return true;
            }
            catch (Exception ex)
            {
                Console.WriteLine($"Set Velocity: {vel}, Error: {ex.Message}");
                return false;
            }
        }

        public bool SetAxisMode(byte mode)
        {
            try
            {
                master.WriteSingleRegister(ModbusId, Adrress.AxisMode, mode);
                return true;
            }
            catch (Exception ex)
            {
                Console.WriteLine($"Switch Mode {mode}, Error: {ex.Message}");
                return false;
            }
        }

        public bool Enable(bool on_off)
        {
            ushort cmd = on_off ? ModbusValue.PowerOn : ModbusValue.PowerOff;
            try
            {
                master.WriteSingleRegister(ModbusId, Adrress.ControlWord, cmd);
                return true;
            }
            catch (Exception ex)
            {
                Console.WriteLine($"Enalbe Axis {on_off}, Error: {ex.Message}");
                return false;
            }
        }

        private int GetInt32Value(ushort address)
        {
            try
            {
                int ret = 0;
                ushort[] registers = master.ReadHoldingRegisters(ModbusId, address, 2);
                byte[] bytes_low = BitConverter.GetBytes(registers[0]);
                byte[] bytes_high = BitConverter.GetBytes(registers[1]);
                byte[] bytes = new byte[4] {
                    bytes_low[0],
                    bytes_low[1],
                    bytes_high[0],
                    bytes_high[1]
                };
                ret = BitConverter.ToInt32(bytes, 0);
                return ret;
            }
            catch (Exception ex)
            {
                throw new Exception($"Get Address {address.ToString("X4")} Value Error: {ex.Message}");
            }
        }

        public bool GetActualPos()
        {
            try
            {
                ServoValue.ActualPos = GetInt32Value(Adrress.ActualPos);
                return true;
            }
            catch (Exception ex)
            {
                Console.WriteLine($"Get Actual Postion Error: {ex.Message}");
                return false;
            }
        }

        public bool GetActualCurrent()
        {
            try
            {
                ushort[] registers = master.ReadHoldingRegisters(ModbusId, Adrress.ActualCurrent, 1);
                ServoValue.AcutalCurrent = (short)registers[0] / ModbusValue.CurrentRatio;
                return true;
            }
            catch (Exception ex)
            {
                Console.WriteLine($"Get Actual Current Error: {ex.Message}");
                return false;
            }
        }

        public void Dispose()
        {
            serial.Close();
            serial.Dispose();
            master.Dispose();
        }

        public static ushort LowWord(uint value)
        {
            var ret = value & 0xFFFF;
            return (ushort)ret;
        }

        public static ushort HighWord(uint value)
        {
            var ret = (value & 0xFFFF0000) >> 16;
            return (ushort)ret;
        }
    }
}
